#include <ros/ros.h>
// #include <string.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <unistd.h>


std::string img_path;
std::string node_name;
std::string img_type;
float rate;

int getParam(ros::NodeHandle nh)
{
    node_name = ros::this_node::getName();

    ROS_INFO("node_name: %s", node_name.c_str());
    // std::cout<<node_name<<std::endl;

    nh.param<std::string>(node_name+"/img_path", img_path,"");

    nh.param<std::string>(node_name+"/img_type", img_type,"color");

    nh.param<float>(node_name+"/rate", rate,1.0);

    // std::cout<<img_path<<std::endl;
    ROS_INFO("img_path: %s", img_path.c_str());

    return 0;
}

sensor_msgs::ImagePtr imageRead(const std::string img_path)
{
    cv_bridge::CvImage cv_img;

    cv_img = cv_bridge::CvImage();

    
    if(img_type == "color"){
        cv_img.image = cv::imread(img_path,cv::IMREAD_COLOR);
        cv_img.encoding = sensor_msgs::image_encodings::BGR8;
    }
    else if(img_type == "depth"){
        cv_img.image = cv::imread(img_path,cv::IMREAD_ANYDEPTH);
        cv_img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;  
    }


    return cv_img.toImageMsg();
}

void show_cwd(){

    char buff[250];
    getcwd(buff, 250);
    std::string current_working_directory(buff);
    ROS_INFO("current_working_directory: %s", current_working_directory.c_str());
    return;
}
int main(int argc, char** argv)
{

    show_cwd();

    ros::init(argc, argv,"empty");

    ros::NodeHandle nh;

    getParam(nh);

    ros::Rate naptime(rate);

    image_transport::ImageTransport it(nh);

    image_transport::Publisher image_pub_ = it.advertise(node_name+"/picture", 1);

    sensor_msgs::ImagePtr msg = imageRead(img_path);

    while(ros::ok()){
        image_pub_.publish(msg);
        naptime.sleep(); 
    }

    return 0;
}